#include "Test.h"
#include <cstdlib>
#include <random>
#include <numeric>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/common/transforms.h>
#include <Eigen/Eigen>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include "PoseGrabber.h"
#include "ActionAbout.h"
using namespace pcl;
typedef pcl::PointXYZ PointIoType;

Test::Test()
{
}


Test::~Test()
{
}

void Test::MultinomialResampleTest()
{

	//ParticleFilter mParticleFilter;
		//mParticleFilter.mInitPara.particle_count = PARTICLE_COUNT;
		//mParticleFilter.ParticleInit();
		////mParticleFilter.SetParticle(PARTICLE_COUNT);
		//cout << "WheelResample" << endl;
		////mParticleFilter.WheelResample();
		//cout << "MultinomialResample_Test" << endl;
		//mParticleFilter.MultinomialResample_Test();


	vector<double> orign, output, output_temp, p_weights, weight_temp;
	//load the particle weight and normalize
	double sum = GetSumOfParticle();
	for (int i = 0; i < m_particles.size(); i++) {
		orign.push_back(m_particles[i].log_w);
		p_weights.push_back(m_particles[i].log_w / sum);
	}
	int iterate = 10;
	vector<int> index(p_weights.size(), 0);
	output = orign;
	weight_temp.assign(p_weights.begin(), p_weights.end());
	for (int i = 0; i < iterate; i++) {
		index.clear();
		index = resample_Multinomial(weight_temp);
		for (int j = 0; j < orign.size(); j++) {
			output_temp.push_back(output[index[j]]);
		}
		output = output_temp;
		output_temp.clear();
		double sum = 0;
		for (auto&p : output) {
			sum += p;
		}
		weight_temp.clear();
		for (int k = 0; k < output.size(); k++) {
			weight_temp.push_back(double(output[k] / sum));
		}
		DisplayVector(output);
		//DisplayVector(weight_temp);
	}
}

int Test::KdtreeLineTest()
{
	//pcl viewer
	pcl::visualization::PCLVisualizer viewer("kdtree test");
	//pcl::PointCloud<PointIoType> &model, pcl::PointCloud<PointIoType> &shape
	string model_file = "H:/BaiduNetdiskDownload/07/000000.pcd";
	string shape_file = "H:/BaiduNetdiskDownload/07/000020.pcd";
	//init model and shape pointcloud
	pcl::PointCloud<PointIoType>::Ptr model(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr shape(new pcl::PointCloud<PointIoType>);
	if (pcl::io::loadPCDFile<PointIoType>(model_file, *model) == -1 || pcl::io::loadPCDFile<PointIoType>(model_file, *shape) == -1) {
		PCL_ERROR("Could not load the pcd file, maybe you should check the path");
		return -1;
	}
	//transform the shape pointcloud far away form model so that make the different clear
	 /*  
	| 1 0 0 x |  
	| 0 1 0 y |  
	| 0 0 1 z |  
	| 0 0 0 1 | 
	*/
	Eigen::Affine3f transform = Eigen::Affine3f::Identity();
	transform.translation() << 0.5, 0.9, 0.2;
	//transform.rotate(Eigen::AngleAxisf(M_PI / 4, Eigen::Vector3f::UnitX()));
	printf("\nMethod #2: using an Affine3f\n");
	std::cout << transform.matrix() << std::endl;
	pcl::transformPointCloud(*shape,*shape,transform);

	//use kdtree to find the close point from shape to model
	pcl::KdTreeFLANN<PointIoType> kdtree;

	int k=1;
	kdtree.setInputCloud(model);
	std::vector<int> pointIdNsearch(k);
	std::vector<float> pointSquaredDistance(k);

	for (int i = 0; i < shape->size();i=i+200) {
		PointIoType search_point;
		search_point = shape->points[i];
		if (kdtree.nearestKSearch(search_point,k,pointIdNsearch, pointSquaredDistance)>0) {
			viewer.addLine(search_point,model->points[pointIdNsearch[0]],to_string(i));
		}
	}
	
	pcl::visualization::PointCloudColorHandlerCustom<PointIoType> model_cloud_handler(model,255,0,0);
	viewer.addPointCloud(model,model_cloud_handler,"model");

	pcl::visualization::PointCloudColorHandlerCustom<PointIoType> shape_cloud_handler(shape,0,255,0);
	viewer.addPointCloud(shape,shape_cloud_handler,"shape");

	while (!viewer.wasStopped()) {
		viewer.spinOnce();
	}
	return 0;
}

void Test::groundTruthGrabberTest()
{
	/*PoseGrabber mPoseGrabber;
	mPoseGrabber.getGroudTruthOffLine();*/
}

void Test::actionGrabberTest()
{
	/*ActionAbout mActionAbout;
	PoseGrabber mPoseGrabber;
	mActionAbout.actionGrabberOffLine(mPoseGrabber.getGroudTruthOffLine());*/
}

void Test::actionAdditionDisturbanceTest()
{
	///*ActionAbout mActionAbout;
	//PoseGrabber mPoseGrabber;
	//mActionAbout.actionGrabberOffLine(mPoseGrabber.getGroudTruthOffLine());
	//Pose3D start(mActionAbout.get(0));
	//Pose3D addition = mActionAbout.getActionAddition(1,1);
	//cout << "the origin pose=";
	//cout << addition << endl;
	//cout << "after adding random error pose=";
	//for (int i = 0; i < 50;i++) {
	//	cout << start + addition << endl;
	//}*/
}





void Test::downsamplePointCloud()
{
	vector<Eigen::Matrix4d> transform;
	vector<string> pcd_filename;

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));
	pcl::PointCloud<PointIoType>::Ptr map(new pcl::PointCloud<PointIoType>());
	pcl::PointCloud<PointIoType>::Ptr target(new pcl::PointCloud<PointIoType>());
	pcl::PointCloud<PointIoType>::Ptr source(new pcl::PointCloud<PointIoType>());

	//down sampling the points cloud to decrease the memeory
	pcl::VoxelGrid<PointIoType> filter;
	filter.setInputCloud(map);
	filter.setLeafSize(0.1f, 0.1f, 0.1f);
	filter.filter(*map);
	//show the map
	viewer1->addPointCloud<PointIoType>(map, "map");
	viewer1->removePointCloud("source");
	viewer1->setBackgroundColor(0, 0, 0);
	while (!viewer1->wasStopped())
	{
	viewer1->spinOnce(100);
	boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}
